#include "PID.h"



PID_Struct  PID_current_q,PID_current_d,PID_velocity,PID_angle;


void PID_Init(PID_Struct *PID,float P,float I,float D,float Max_I,float Max_Out)
{
	PID->Kp = P;
	PID->Ki = I;
	PID->Kd = D;
	PID->Max_Integral = Max_I;
	PID->Max_Output = Max_Out;
	PID->error_prev = 0;
	
}


float PID_Controller(PID_Struct *PID,float error)
{
	float output;
	static float error_prev;
	
	PID->integral = PID->integral + PID->Ki * error;
	PID->integral = _constrain(PID->integral, -PID->Max_Integral, PID->Max_Integral);
	
	output = PID->Kp * error + PID->integral + PID->Kd * (error - error_prev);
	output = _constrain(output, -PID->Max_Output, PID->Max_Output);
	
	error_prev = error;
	
	return output;
}





















